/**
  ******************************************************************************
  * File Name          : sonar_driver.cpp
  * Description        : sonar_driver program body
  ******************************************************************************
  *
  * Copyright (c) 2019 HopeMotion Co., Ltd. 
  * All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without 
  * modification, are permitted, provided that the following conditions are met:
  *
  * 1. Redistribution of source code must retain the above copyright notice, 
  *    this list of conditions and the following disclaimer.
  * 2. Redistributions in binary form must reproduce the above copyright notice,
  *    this list of conditions and the following disclaimer in the documentation
  *    and/or other materials provided with the distribution.
  * 3. Neither the name of HopeMotion nor the names of other 
  *    contributors to this software may be used to endorse or promote products 
  *    derived from this software without specific written permission.
  * 4. This software, including modifications and/or derivative works of this 
  *    software, must execute solely and exclusively on microcontroller or
  *    microprocessor devices manufactured by or for HopeMotion.
  * 5. Redistribution and use of this software other than as permitted under 
  *    this license is void and will automatically terminate your rights under 
  *    this license. 
  *
  * THIS SOFTWARE IS PROVIDED BY HOPEMOTION AND CONTRIBUTORS "AS IS" 
  * AND ANY EXPRESS, IMPLIED OR STATUTORY WARRANTIES, INCLUDING, BUT NOT 
  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 
  * PARTICULAR PURPOSE AND NON-INFRINGEMENT OF THIRD PARTY INTELLECTUAL PROPERTY
  * RIGHTS ARE DISCLAIMED TO THE FULLEST EXTENT PERMITTED BY LAW. IN NO EVENT 
  * SHALL HOPEMOTION OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
  * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, 
  * OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 
  * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 
  * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE,
  * EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  *
  ******************************************************************************
  */
/* Includes ------------------------------------------------------------------*/
#include <sonar_driver/sonar_driver.h>

#if _DYP_A05_
/* Private variables ---------------------------------------------------------*/
static int addr = 0;
static int error = 0;
static int fdcom = 0;
static PortInfo_t portinfo = {9600, 8, 0, 0, 1, 0};

/*
=========================================
|   header   |  id  |  指令  |  校验和  |
-----------------------------------------
| 0x55 0xaa |  0x01  |  0x01 |   0x01   |
=========================================
*/
static unsigned char tx_buffer[TX_BUFFER_MAX] = {0x55, 0xaa, 0x01, 0x01, 0x01};

/*
=========================================
|  帧头     |  地址  |  指令  |  校验和  |
-----------------------------------------
| 0x55 0xaa |  0x01  |  0x01 |   0x01   |
=========================================
*/
static unsigned char rx_buffer[RX_BUFFER_MAX];

static unsigned int  distance[4] = {0, 0, 0, 0};

/* Private function prototypes -----------------------------------------------*/
/**
  * @brief
  * @param
  * @retval
  */
int data_check(unsigned char buf[RX_BUFFER_MAX])
{
    int checksum = 0;
    if((buf[0]==0x55)&&(buf[1]==0xAA))
    {
        for(int i=0; i<RX_BUFFER_MAX-1; i++)
        {
            checksum += buf[i];
        }
        if((checksum&0x00ff)== buf[RX_BUFFER_MAX-1])
        {
            return 1;
        }
    }
    printf("板卡%d数据接收错误.\r\n", addr);
    error = 2;
    return 0;
}

/**
  * @brief
  * @param
  * @retval
  */
unsigned int data_merge(unsigned short dt_h, unsigned short dt_l)
{
    unsigned int tmp;
    tmp = dt_h;
    tmp <<= 8;
    tmp |= dt_l;
    return tmp;
}
#else
/* Private variables ---------------------------------------------------------*/

/* Private function prototypes -----------------------------------------------*/
#endif

/**
  * @brief
  * @param
  * @retval
  */
int main(int argc, char**argv)
{
    ros::init(argc, argv, "sonar_driver");
    ros::NodeHandle nh("~");
    ros::Rate loop_rate(2);

#if _DYP_A05_
    //获取动态参数
    std::string ns_1;
    std::string ns_2;
    std::string ns_3;
    std::string ns_4;
    std::string port;
    

    nh.getParam("sonar_addr", addr);
    nh.getParam("sonar_port", port);
    nh.getParam("sonar_ns_1", ns_1);
    nh.getParam("sonar_ns_2", ns_2);
    nh.getParam("sonar_ns_3", ns_3);
    nh.getParam("sonar_ns_4", ns_4);
    

    //根据输入修改发送帧
    unsigned int cs = 0; 
    tx_buffer[2] = addr;
    tx_buffer[TX_BUFFER_MAX-1] = 0;
    printf("[板卡%d查询命令]: ", addr);
    for(int j=0; j<TX_BUFFER_MAX-1; j++)
    {
        printf("0x%02X ", tx_buffer[j]);
        cs += tx_buffer[j];
    }
    tx_buffer[TX_BUFFER_MAX-1] = cs&0x00ff;
    printf("0x%02X \r\n", tx_buffer[TX_BUFFER_MAX-1]);

    //设置输出的超声波雷达话题
    sensor_msgs::Range  sonar1;
    sensor_msgs::Range  sonar2;
    sensor_msgs::Range  sonar3;
    sensor_msgs::Range  sonar4;
    ros::Publisher pub1 = nh.advertise<sensor_msgs::Range>("sonar_1", 10);
    ros::Publisher pub2 = nh.advertise<sensor_msgs::Range>("sonar_2", 10);
    ros::Publisher pub3 = nh.advertise<sensor_msgs::Range>("sonar_3", 10);
    ros::Publisher pub4 = nh.advertise<sensor_msgs::Range>("sonar_4", 10);

    //设备初始化
    fdcom = uart_init(port.c_str());
    if(fdcom > 0)
    {
        uart_set(fdcom, &portinfo);
        printf("[板卡%d通信参数]: %d, 8, 0, 0, 1, 0.\n",addr,9600);
    }else{
        printf("[板卡%d错误提示]: 串口打开错误.\n",addr);
        error = 1;
        //return 1;
    }

    while (ros::ok())
    {
        if(error)
        {
            distance[0] = 0;
            distance[1] = 0;
            distance[2] = 0;
            distance[3] = 0;
        }
        else
        {
            uart_txd(fdcom, tx_buffer, TX_BUFFER_MAX);
            usleep(500000);
            uart_rxd(fdcom, rx_buffer, RX_BUFFER_MAX, portinfo.baudrate);
/*
            printf("recv: ");
            for(int i=0; i<13; i++)
            {
                printf("0x%02X ",rx_buffer[i]);
            }
            printf("\r\n");
*/
            if(data_check(rx_buffer))
            {
                distance[0] = data_merge(rx_buffer[10], rx_buffer[11]);
                distance[1] = data_merge(rx_buffer[8], rx_buffer[9]);
                distance[2] = data_merge(rx_buffer[6], rx_buffer[7]);
                distance[3] = data_merge(rx_buffer[4], rx_buffer[5]);
            }
        }
        //雷达1
        sonar1.header.frame_id = ns_1;
        sonar1.radiation_type = sensor_msgs::Range::ULTRASOUND;
        sonar1.field_of_view = 1.047;
        sonar1.min_range = 0.2;
        sonar1.max_range = 4.0;
        sonar1.range = distance[0];
        pub1.publish(sonar1); 
        //雷达2
        sonar2.header.frame_id = ns_2;
        sonar2.radiation_type = sensor_msgs::Range::ULTRASOUND;
        sonar2.field_of_view = 1.047;
        sonar2.min_range = 0.2;
        sonar2.max_range = 4.0;
        sonar2.range = distance[1];
        pub2.publish(sonar2); 
        //雷达3
        sonar3.header.frame_id = ns_3;
        sonar3.radiation_type = sensor_msgs::Range::ULTRASOUND;
        sonar3.field_of_view = 1.047;
        sonar3.min_range = 0.2;
        sonar3.max_range = 4.0;
        sonar3.range = distance[2];
        pub3.publish(sonar3); 
        //雷达4
        sonar4.header.frame_id = ns_4;
        sonar4.radiation_type = sensor_msgs::Range::ULTRASOUND;
        sonar4.field_of_view = 1.047;
        sonar4.min_range = 0.2;
        sonar4.max_range = 4.0;
        sonar4.range = distance[3];
        pub4.publish(sonar4); 
        
        ros::spinOnce();
        loop_rate.sleep();
    }
    uart_deinit(fdcom);
#else
    while (ros::ok())
    {
        ros::spinOnce();
        loop_rate.sleep();  
    }
#endif
    return 0;
}

/************************ (C) COPYRIGHT HopeMotion *****END OF FILE****/

